Make GPGSA more robust for short sentences, more C++-like. Add warning for badk check...

diff --git a/nmea.cc b/nmea.cc
index 150f685193e3591d78f3948805006d508eaa6bed..e259d2f577717e053c351f30dded979bc9303041 100644 (file)
--- a/nmea.cc
+++ b/nmea.cc
@@ -26,6 +26,7 @@
 #include "gbser.h"
 #include "strptime.h"
 #include "jeeps/gpsmath.h"
+#include "src/core/logging.h"
 
 #include <ctype.h>
 #include <math.h>
@@ -69,12 +70,12 @@
    ' $GPRMC - Recommended minimum specific GNSS Data
    ' $GPRMC,085721.194,A,5917.7210,N,01103.9227,E,21.42,50.33,300504,,*07
    '  2    085721       Fix taken at 08:57:21 UTC
-   '  3    A                           Fix valid (this field reads V if fix is not valid)
+   '  3    A           Fix valid (this field reads V if fix is not valid)
    '  4,5  5917.7210,N   Latitude 59 deg 17.7210' N
    '  6,7  01103.9227,E  Longitude 11 deg 03.9227' E
-   '  8    21.42                       Speed over ground (knots)
-   '  9    50.33                       Course over ground (true)
-   '   10   300504                     Date 30/05-2004
+   '  8    21.42       Speed over ground (knots)
+   '  9    50.33       Course over ground (true)
+   ' 10   300504       Date 30/05-2004
    '  11   Empty field Magnetic variation
 
          GSA - GPS DOP and active satellites
@@ -505,9 +506,9 @@ gpgga_parse(char* ibuf)
 
   WAYPT_SET(waypt, geoidheight, geoidheight);
 
-  waypt->sat   = nsats;
+  waypt->sat = nsats;
 
-  waypt->hdop  = hdop;
+  waypt->hdop = hdop;
 
   switch (fix) {
   case 0:
@@ -678,49 +679,48 @@ gpzda_parse(char* ibuf)
   // This can't have worked.
 }
 
+// This function has had a hard life. It began as a moderately terrifying
+// bunch of nested sscanfs. In 2017, robertl replaced the scanf stuff with
+// a QString::split() to make it more tolerant of really empty fields from
+// certain GPS implementations, but didn't replace the (somewhat funky) back
+// half to match the parse. There are definitely some readibility issues
+// here.
+// The numbering as per http://aprs.gids.nl/nmea/#gsa was the reference as
+// the field numbers conveniently match our index.
 static void
 gpgsa_parse(char* ibuf)
 {
-  char fixauto;
-  char fix;
   int  prn[12] = {0};
-  int  scn,cnt;
-  float pdop=0,hdop=0,vdop=0;
-  char*        tok=0;
-
   memset(prn,0xff,sizeof(prn));
 
-  scn = sscanf(ibuf,"$%*2cGSA,%c,%c,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",
-               &fixauto, &fix,
-               &prn[0],&prn[1],&prn[2],&prn[3],&prn[4],&prn[5],
-               &prn[6],&prn[7],&prn[8],&prn[9],&prn[10],&prn[11]);
-  if (scn < 2) {
-    warning(MYNAME ": Short GSA sentence.\n");
-  }
-  /*
-       sscanf has scanned all the leftmost elements
-       we'll rescan by skipping 15 commas to the dops
-  */
-  tok = ibuf;
-  for (cnt=0; (tok)&&(cnt<15); cnt++) {
-    tok = strchr(tok,',');
-    if (!tok) {
-      break;
-    }
-    tok++;
+  QStringList fields = QString(ibuf).split(",", QString::KeepEmptyParts);
+  int nfields = fields.size();
+  // 0 = "GPGSA"
+  // 1 = Mode. Ignored
+  QChar fix;
+  if (nfields > 1) {
+    fix = fields[2][0];
   }
-  if (tok) {
-    sscanf(tok,"%f,%f,%f",&pdop,&hdop,&vdop);
+  // 12 fields, index 3 through 14. 
+  for (int cnt = 0; cnt <= 11; cnt++) {
+    if (nfields >= cnt + 2) prn[cnt] = fields[cnt + 3].toInt();
   }
 
+  float pdop = 0, hdop = 0, vdop = 0;
+  if (nfields > 14) pdop = fields[15].toDouble();
+  if (nfields > 15) hdop = fields[16].toDouble();
+  if (nfields > 16) {
+     // Last one is special. The checksum isn't split out above.
+    fields[17].chop(3);
+    vdop = fields[17].toDouble();
+  }
 
   if (curr_waypt) {
-
     if (curr_waypt->fix!=fix_dgps) {
-      if       (fix=='3')      {
-        curr_waypt->fix=fix_3d;
-      } else if (fix=='2')     {
-        curr_waypt->fix=fix_2d;
+      if (fix=='3') {
+        curr_waypt->fix = fix_3d;
+      } else if (fix=='2') {
+        curr_waypt->fix = fix_2d;
       }
     }
 
@@ -728,9 +728,9 @@ gpgsa_parse(char* ibuf)
     curr_waypt->hdop = hdop;
     curr_waypt->vdop = vdop;
 
-    if (curr_waypt->sat  <= 0) {
-      for (cnt=0; cnt<12; cnt++) {
-        curr_waypt->sat += (prn[cnt]>0)?(1):(0);
+    if (curr_waypt->sat <= 0) {
+      for (int cnt = 0; cnt <= 11; cnt++) {
+        curr_waypt->sat += (prn[cnt] > 0) ? 1 : 0;
       }
     }
   }
@@ -838,7 +838,7 @@ pcmpt_parse(char* ibuf)
     /*
      * Since we oh-so-cleverly inserted points at the head,
      * we can rip through the queue forward now to get our
-    `           * handy-dandy reversing effect.
+     * handy-dandy reversing effect.
      */
     trk_head = route_head_alloc();
     track_add_head(trk_head);
@@ -873,7 +873,8 @@ nmea_fix_timestamps(route_head* track)
       Waypoint* wpt = (Waypoint*)elem;
 
       wpt->creation_time += delta_tm;
-      if ((prev != NULL) && (prev->creation_time > wpt->creation_time)) {      /* go over midnight ? */
+      if ((prev != NULL) && (prev->creation_time > wpt->creation_time)) {
+        /* go over midnight ? */
         delta_tm += SECONDS_PER_DAY;
         wpt->creation_time += SECONDS_PER_DAY;
       }
@@ -883,7 +884,7 @@ nmea_fix_timestamps(route_head* track)
     time_t prev;
     queue* elem;
 
-    tm.tm_hour = 23;   /* last date found */
+    tm.tm_hour = 23; /* last date found */
     tm.tm_min = 59;
     tm.tm_sec = 59;
 
@@ -897,7 +898,7 @@ nmea_fix_timestamps(route_head* track)
       if (wpt->wpt_flags.fmt_use != 0) {
         time_t dt;
 
-        wpt->wpt_flags.fmt_use = 0;    /* reset flag */
+        wpt->wpt_flags.fmt_use = 0; /* reset flag */
 
         dt = (prev / SECONDS_PER_DAY) * SECONDS_PER_DAY;
         wpt->creation_time += dt;
@@ -951,10 +952,7 @@ nmea_parse_one_line(char* ibuf)
     ck++;
     sscanf(ck, "%2X", &ckcmp);
     if (ckval != ckcmp) {
-#if 0
-      printf("ckval %X, %X, %s\n", ckval, ckcmp, ck);
-      printf("NMEA %s\n", tbuf);
-#endif
+      Warning() << "Invalid NMEA checksum. Computed " << ckval << " but found " << ckcmp << ". Ignoring sentence";
       return;
     }
 
@@ -1326,7 +1324,7 @@ nmea_trackpt_pr(const Waypoint* wpt)
 
     /* GISTeq doesn't care about the checksum, but wants this prefixed, so
      * we can write it with abandon.
-        */
+     */
     if (opt_gisteq) {
       gbfprintf(file_out, "---,");
     }
@@ -1402,7 +1400,7 @@ nmea_wr_posn(Waypoint* wpt)
 static void
 nmea_wr_posn_deinit(void)
 {
-//     nmea_wr_deinit();
+// nmea_wr_deinit();
 }
 
 
@@ -1421,7 +1419,7 @@ ff_vecs_t nmea_vecs = {
   nmea_write,
   NULL,
   nmea_args,
-  CET_CHARSET_ASCII, 0,        /* CET-REVIEW */
+  CET_CHARSET_ASCII, 0, /* CET-REVIEW */
   {
     nmea_rd_posn_init, nmea_rd_posn, nmea_rd_deinit,
     nmea_wr_posn_init, nmea_wr_posn, nmea_wr_posn_deinit